/*
 * @Description: 
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:12:01
 * @LastEditTime: 2021-09-20 14:38:05
 * @LastEditors: Sang Hao
 */

#include "lidar_slam/publisher/odometry_publisher.hpp"

namespace lidar_slam {
OdometryPublisher::OdometryPublisher(ros::NodeHandle& nh, 
									std::string topic_name, 
									std::string base_frame_id, 
									std::string child_frame_id, 
									size_t buff_size) 
	:nh_(nh) {
	publisher_ = nh.advertise<nav_msgs::Odometry>(topic_name, buff_size);
	odometry_.header.frame_id = base_frame_id;
	odometry_.child_frame_id = child_frame_id;
}

/**
 * @description: 将位姿矩阵和时间整合到里程计odometry_中，并发布出去
 * @param  {*}
 * @return {*}
 * @param {Time} time
 */
void OdometryPublisher::PublishData(const Eigen::Matrix4f& transform_matrix, ros::Time time) {
	odometry_.header.stamp = time;
	
	odometry_.pose.pose.position.x = transform_matrix(0, 3);
	odometry_.pose.pose.position.y = transform_matrix(1, 3);
	odometry_.pose.pose.position.z = transform_matrix(2, 3);

	Eigen::Quaternionf q;
	q = transform_matrix.block<3, 3>(0, 0);
	odometry_.pose.pose.orientation.x = q.x();
	odometry_.pose.pose.orientation.y = q.y();
	odometry_.pose.pose.orientation.z = q.z();
	odometry_.pose.pose.orientation.w = q.w();

	publisher_.publish(odometry_);
}

/**
 * @description: 编写对外接口，调用私有成员函数PublishData，方便外部使用
 * @param  {*}
 * @return {*}
 * @param {double} time
 */
void OdometryPublisher::Publish(const Eigen::Matrix4f& transform_matrix, double time) {
	ros::Time ros_time((float)time);
	PublishData(transform_matrix, ros_time);
}
void OdometryPublisher::Publish(const Eigen::Matrix4f& transform_matrix) {
	PublishData(transform_matrix, ros::Time::now());
}

bool OdometryPublisher::HasSubscribers() {
	return publisher_.getNumSubscribers() != 0;
}

}

